%#codegen
function nextP  = CovariancePrediction(deltaAngle, ...
    deltaVelocity, ...
    states,...
    P, ...  % Previous state covariance matrix
    dt) % IMU and prediction time step


    
    dAngBiasSigma = single(dt*0.05/3600*pi/180);
    dVelBiasSigma = single(dt*0.01/60);
%     magEarthSigma = single(dt*10/60); % 10 mgauss per minute
%     magBodySigma  = single(dt*100/60); % 100 mgauss per minute - allow to adapt more rapidly due to changes in airframe
    
    processNoise = [1e-9*ones(1,10), dAngBiasSigma*[1 1 1], dVelBiasSigma*[1 1 1]].^2;
    
    % Specify the estimated errors on the delta angles and delta velocities
    daxCov = (dt*25.0/60*pi/180)^2;
    dayCov = (dt*25.0/60*pi/180)^2;
    dazCov = (dt*25.0/60*pi/180)^2;
    dvxCov = (dt*0.5)^2;
    dvyCov = (dt*0.5)^2;
    dvzCov = (dt*0.5)^2;
    
    dvx = deltaVelocity(1);
    dvy = deltaVelocity(2);
    dvz = deltaVelocity(3);
    dax = deltaAngle(1);
    day = deltaAngle(2);
    daz = deltaAngle(3);
    
    q0 = states(1);
    q1 = states(2);
    q2 = states(3);
    q3 = states(4);
    
%     vn = states(5);
%     ve = states(6);
%     vd = states(7);
%     
%     pn = states(8);
%     pe = states(9);
%     pd = states(10);
    
    dax_b = states(11);
    day_b = states(12);
    daz_b = states(13);
    
    dvx_b = states(14);
    dvy_b = states(15);
    dvz_b = states(16);
    
%     
%     magN = states(17);
%     magE = states(18);
%     magD = states(19);
%     
%     magX = states(20);
%     magY = states(21);
%     magZ = states(22);
    
    % Predicted covariance
SF = zeros(14,1);
SF(1) = dvz - dvz_b;
SF(2) = dvy - dvy_b;
SF(3) = dvx - dvx_b;
SF(4) = 2*q1*SF(3) + 2*q2*SF(2) + 2*q3*SF(1);
SF(5) = 2*q0*SF(2) - 2*q1*SF(1) + 2*q3*SF(3);
SF(6) = 2*q0*SF(3) + 2*q2*SF(1) - 2*q3*SF(2);
SF(7) = day/2 - day_b/2;
SF(8) = daz/2 - daz_b/2;
SF(9) = dax/2 - dax_b/2;
SF(10) = 2*q1*SF(2);
SF(11) = 2*q0*SF(1);
SF(12) = q1/2;
SF(13) = q2/2;
SF(14) = q3/2;

SG = zeros(8,1);
SG(1) = q0/2;
SG(2) = q3^2;
SG(3) = q2^2;
SG(4) = q1^2;
SG(5) = q0^2;
SG(6) = 2*q2*q3;
SG(7) = 2*q1*q3;
SG(8) = 2*q1*q2;

SQ = zeros(11,1);
SQ(1) = dvzCov*(SG(6) - 2*q0*q1)*(SG(2) - SG(3) - SG(4) + SG(5)) - dvyCov*(SG(6) + 2*q0*q1)*(SG(2) - SG(3) + SG(4) - SG(5)) + dvxCov*(SG(7) - 2*q0*q2)*(SG(8) + 2*q0*q3);
SQ(2) = dvzCov*(SG(7) + 2*q0*q2)*(SG(2) - SG(3) - SG(4) + SG(5)) - dvxCov*(SG(7) - 2*q0*q2)*(SG(2) + SG(3) - SG(4) - SG(5)) + dvyCov*(SG(6) + 2*q0*q1)*(SG(8) - 2*q0*q3);
SQ(3) = dvzCov*(SG(6) - 2*q0*q1)*(SG(7) + 2*q0*q2) - dvyCov*(SG(8) - 2*q0*q3)*(SG(2) - SG(3) + SG(4) - SG(5)) - dvxCov*(SG(8) + 2*q0*q3)*(SG(2) + SG(3) - SG(4) - SG(5));
SQ(4) = (dayCov*q1*SG(1))/2 - (dazCov*q1*SG(1))/2 - (daxCov*q2*q3)/4;
SQ(5) = (dazCov*q2*SG(1))/2 - (daxCov*q2*SG(1))/2 - (dayCov*q1*q3)/4;
SQ(6) = (daxCov*q3*SG(1))/2 - (dayCov*q3*SG(1))/2 - (dazCov*q1*q2)/4;
SQ(7) = (daxCov*q1*q2)/4 - (dazCov*q3*SG(1))/2 - (dayCov*q1*q2)/4;
SQ(8) = (dazCov*q1*q3)/4 - (daxCov*q1*q3)/4 - (dayCov*q2*SG(1))/2;
SQ(9) = (dayCov*q2*q3)/4 - (daxCov*q1*SG(1))/2 - (dazCov*q2*q3)/4;
SQ(10) = SG(1)^2;

SPP = zeros(8,1);
SPP(1) = SF(10) + SF(11) - 2*q2*SF(3);
SPP(2) = q0^2 - q1^2 + q2^2 - q3^2;
SPP(3) = q0^2 + q1^2 - q2^2 - q3^2;
SPP(4) = 2*q0*q2 - 2*q1*q3;
SPP(5) = 2*q0*q1 - 2*q2*q3;
SPP(6) = 2*q0*q3 - 2*q1*q2;
SPP(7) = SF(8);
SPP(8) = SF(9);

nextP = zeros(16,16);
nextP(1,1) = P(1,1) - P(3,1)*SF(7) + P(11,1)*SF(12) + P(12,1)*SF(13) + P(13,1)*SF(14) - P(2,1)*SPP(8) - P(4,1)*SPP(7) + (daxCov*SQ(11))/4 - SF(7)*(P(1,3) - P(3,3)*SF(7) + P(11,3)*SF(12) + P(12,3)*SF(13) + P(13,3)*SF(14) - P(2,3)*SPP(8) - P(4,3)*SPP(7)) + SF(12)*(P(1,11) - P(3,11)*SF(7) + P(11,11)*SF(12) + P(12,11)*SF(13) + P(13,11)*SF(14) - P(2,11)*SPP(8) - P(4,11)*SPP(7)) + SF(13)*(P(1,12) - P(3,12)*SF(7) + P(11,12)*SF(12) + P(12,12)*SF(13) + P(13,12)*SF(14) - P(2,12)*SPP(8) - P(4,12)*SPP(7)) + SF(14)*(P(1,13) - P(3,13)*SF(7) + P(11,13)*SF(12) + P(12,13)*SF(13) + P(13,13)*SF(14) - P(2,13)*SPP(8) - P(4,13)*SPP(7)) - SPP(8)*(P(1,2) - P(3,2)*SF(7) + P(11,2)*SF(12) + P(12,2)*SF(13) + P(13,2)*SF(14) - P(2,2)*SPP(8) - P(4,2)*SPP(7)) - SPP(7)*(P(1,4) - P(3,4)*SF(7) + P(11,4)*SF(12) + P(12,4)*SF(13) + P(13,4)*SF(14) - P(2,4)*SPP(8) - P(4,4)*SPP(7)) + (dayCov*q2^2)/4 + (dazCov*q3^2)/4;
nextP(1,2) = P(1,2) + SQ(9) - P(3,2)*SF(7) + P(11,2)*SF(12) + P(12,2)*SF(13) + P(13,2)*SF(14) - P(2,2)*SPP(8) - P(4,2)*SPP(7) - SF(7)*(P(1,4) - P(3,4)*SF(7) + P(11,4)*SF(12) + P(12,4)*SF(13) + P(13,4)*SF(14) - P(2,4)*SPP(8) - P(4,4)*SPP(7)) + SF(14)*(P(1,12) - P(3,12)*SF(7) + P(11,12)*SF(12) + P(12,12)*SF(13) + P(13,12)*SF(14) - P(2,12)*SPP(8) - P(4,12)*SPP(7)) - SF(13)*(P(1,13) - P(3,13)*SF(7) + P(11,13)*SF(12) + P(12,13)*SF(13) + P(13,13)*SF(14) - P(2,13)*SPP(8) - P(4,13)*SPP(7)) + SPP(8)*(P(1,1) - P(3,1)*SF(7) + P(11,1)*SF(12) + P(12,1)*SF(13) + P(13,1)*SF(14) - P(2,1)*SPP(8) - P(4,1)*SPP(7)) + SPP(7)*(P(1,3) - P(3,3)*SF(7) + P(11,3)*SF(12) + P(12,3)*SF(13) + P(13,3)*SF(14) - P(2,3)*SPP(8) - P(4,3)*SPP(7)) - (q0*(P(1,11) - P(3,11)*SF(7) + P(11,11)*SF(12) + P(12,11)*SF(13) + P(13,11)*SF(14) - P(2,11)*SPP(8) - P(4,11)*SPP(7)))/2;
nextP(2,2) = P(2,2) - P(4,2)*SF(7) + P(12,2)*SF(14) - P(13,2)*SF(13) + P(1,2)*SPP(8) + P(3,2)*SPP(7) + daxCov*SQ(10) - (P(11,2)*q0)/2 - SF(7)*(P(2,4) - P(4,4)*SF(7) + P(12,4)*SF(14) - P(13,4)*SF(13) + P(1,4)*SPP(8) + P(3,4)*SPP(7) - (P(11,4)*q0)/2) + SF(14)*(P(2,12) - P(4,12)*SF(7) + P(12,12)*SF(14) - P(13,12)*SF(13) + P(1,12)*SPP(8) + P(3,12)*SPP(7) - (P(11,12)*q0)/2) - SF(13)*(P(2,13) - P(4,13)*SF(7) + P(12,13)*SF(14) - P(13,13)*SF(13) + P(1,13)*SPP(8) + P(3,13)*SPP(7) - (P(11,13)*q0)/2) + SPP(8)*(P(2,1) - P(4,1)*SF(7) + P(12,1)*SF(14) - P(13,1)*SF(13) + P(1,1)*SPP(8) + P(3,1)*SPP(7) - (P(11,1)*q0)/2) + SPP(7)*(P(2,3) - P(4,3)*SF(7) + P(12,3)*SF(14) - P(13,3)*SF(13) + P(1,3)*SPP(8) + P(3,3)*SPP(7) - (P(11,3)*q0)/2) + (dayCov*q3^2)/4 + (dazCov*q2^2)/4 - (q0*(P(2,11) - P(4,11)*SF(7) + P(12,11)*SF(14) - P(13,11)*SF(13) + P(1,11)*SPP(8) + P(3,11)*SPP(7) - (P(11,11)*q0)/2))/2;
nextP(1,3) = P(1,3) + SQ(8) - P(3,3)*SF(7) + P(11,3)*SF(12) + P(12,3)*SF(13) + P(13,3)*SF(14) - P(2,3)*SPP(8) - P(4,3)*SPP(7) + SF(7)*(P(1,1) - P(3,1)*SF(7) + P(11,1)*SF(12) + P(12,1)*SF(13) + P(13,1)*SF(14) - P(2,1)*SPP(8) - P(4,1)*SPP(7)) - SF(14)*(P(1,11) - P(3,11)*SF(7) + P(11,11)*SF(12) + P(12,11)*SF(13) + P(13,11)*SF(14) - P(2,11)*SPP(8) - P(4,11)*SPP(7)) + SF(12)*(P(1,13) - P(3,13)*SF(7) + P(11,13)*SF(12) + P(12,13)*SF(13) + P(13,13)*SF(14) - P(2,13)*SPP(8) - P(4,13)*SPP(7)) - SPP(7)*(P(1,2) - P(3,2)*SF(7) + P(11,2)*SF(12) + P(12,2)*SF(13) + P(13,2)*SF(14) - P(2,2)*SPP(8) - P(4,2)*SPP(7)) + SPP(8)*(P(1,4) - P(3,4)*SF(7) + P(11,4)*SF(12) + P(12,4)*SF(13) + P(13,4)*SF(14) - P(2,4)*SPP(8) - P(4,4)*SPP(7)) - (q0*(P(1,12) - P(3,12)*SF(7) + P(11,12)*SF(12) + P(12,12)*SF(13) + P(13,12)*SF(14) - P(2,12)*SPP(8) - P(4,12)*SPP(7)))/2;
nextP(2,3) = P(2,3) + SQ(6) - P(4,3)*SF(7) + P(12,3)*SF(14) - P(13,3)*SF(13) + P(1,3)*SPP(8) + P(3,3)*SPP(7) - (P(11,3)*q0)/2 + SF(7)*(P(2,1) - P(4,1)*SF(7) + P(12,1)*SF(14) - P(13,1)*SF(13) + P(1,1)*SPP(8) + P(3,1)*SPP(7) - (P(11,1)*q0)/2) - SF(14)*(P(2,11) - P(4,11)*SF(7) + P(12,11)*SF(14) - P(13,11)*SF(13) + P(1,11)*SPP(8) + P(3,11)*SPP(7) - (P(11,11)*q0)/2) + SF(12)*(P(2,13) - P(4,13)*SF(7) + P(12,13)*SF(14) - P(13,13)*SF(13) + P(1,13)*SPP(8) + P(3,13)*SPP(7) - (P(11,13)*q0)/2) - SPP(7)*(P(2,2) - P(4,2)*SF(7) + P(12,2)*SF(14) - P(13,2)*SF(13) + P(1,2)*SPP(8) + P(3,2)*SPP(7) - (P(11,2)*q0)/2) + SPP(8)*(P(2,4) - P(4,4)*SF(7) + P(12,4)*SF(14) - P(13,4)*SF(13) + P(1,4)*SPP(8) + P(3,4)*SPP(7) - (P(11,4)*q0)/2) - (q0*(P(2,12) - P(4,12)*SF(7) + P(12,12)*SF(14) - P(13,12)*SF(13) + P(1,12)*SPP(8) + P(3,12)*SPP(7) - (P(11,12)*q0)/2))/2;
nextP(3,3) = P(3,3) + P(1,3)*SF(7) - P(11,3)*SF(14) + P(13,3)*SF(12) - P(2,3)*SPP(7) + P(4,3)*SPP(8) + dayCov*SQ(10) + (dazCov*SQ(11))/4 - (P(12,3)*q0)/2 + SF(7)*(P(3,1) + P(1,1)*SF(7) - P(11,1)*SF(14) + P(13,1)*SF(12) - P(2,1)*SPP(7) + P(4,1)*SPP(8) - (P(12,1)*q0)/2) - SF(14)*(P(3,11) + P(1,11)*SF(7) - P(11,11)*SF(14) + P(13,11)*SF(12) - P(2,11)*SPP(7) + P(4,11)*SPP(8) - (P(12,11)*q0)/2) + SF(12)*(P(3,13) + P(1,13)*SF(7) - P(11,13)*SF(14) + P(13,13)*SF(12) - P(2,13)*SPP(7) + P(4,13)*SPP(8) - (P(12,13)*q0)/2) - SPP(7)*(P(3,2) + P(1,2)*SF(7) - P(11,2)*SF(14) + P(13,2)*SF(12) - P(2,2)*SPP(7) + P(4,2)*SPP(8) - (P(12,2)*q0)/2) + SPP(8)*(P(3,4) + P(1,4)*SF(7) - P(11,4)*SF(14) + P(13,4)*SF(12) - P(2,4)*SPP(7) + P(4,4)*SPP(8) - (P(12,4)*q0)/2) + (daxCov*q3^2)/4 - (q0*(P(3,12) + P(1,12)*SF(7) - P(11,12)*SF(14) + P(13,12)*SF(12) - P(2,12)*SPP(7) + P(4,12)*SPP(8) - (P(12,12)*q0)/2))/2;
nextP(1,4) = P(1,4) + SQ(7) - P(3,4)*SF(7) + P(11,4)*SF(12) + P(12,4)*SF(13) + P(13,4)*SF(14) - P(2,4)*SPP(8) - P(4,4)*SPP(7) + SF(7)*(P(1,2) - P(3,2)*SF(7) + P(11,2)*SF(12) + P(12,2)*SF(13) + P(13,2)*SF(14) - P(2,2)*SPP(8) - P(4,2)*SPP(7)) + SF(13)*(P(1,11) - P(3,11)*SF(7) + P(11,11)*SF(12) + P(12,11)*SF(13) + P(13,11)*SF(14) - P(2,11)*SPP(8) - P(4,11)*SPP(7)) - SF(12)*(P(1,12) - P(3,12)*SF(7) + P(11,12)*SF(12) + P(12,12)*SF(13) + P(13,12)*SF(14) - P(2,12)*SPP(8) - P(4,12)*SPP(7)) + SPP(7)*(P(1,1) - P(3,1)*SF(7) + P(11,1)*SF(12) + P(12,1)*SF(13) + P(13,1)*SF(14) - P(2,1)*SPP(8) - P(4,1)*SPP(7)) - SPP(8)*(P(1,3) - P(3,3)*SF(7) + P(11,3)*SF(12) + P(12,3)*SF(13) + P(13,3)*SF(14) - P(2,3)*SPP(8) - P(4,3)*SPP(7)) - (q0*(P(1,13) - P(3,13)*SF(7) + P(11,13)*SF(12) + P(12,13)*SF(13) + P(13,13)*SF(14) - P(2,13)*SPP(8) - P(4,13)*SPP(7)))/2;
nextP(2,4) = P(2,4) + SQ(5) - P(4,4)*SF(7) + P(12,4)*SF(14) - P(13,4)*SF(13) + P(1,4)*SPP(8) + P(3,4)*SPP(7) - (P(11,4)*q0)/2 + SF(7)*(P(2,2) - P(4,2)*SF(7) + P(12,2)*SF(14) - P(13,2)*SF(13) + P(1,2)*SPP(8) + P(3,2)*SPP(7) - (P(11,2)*q0)/2) + SF(13)*(P(2,11) - P(4,11)*SF(7) + P(12,11)*SF(14) - P(13,11)*SF(13) + P(1,11)*SPP(8) + P(3,11)*SPP(7) - (P(11,11)*q0)/2) - SF(12)*(P(2,12) - P(4,12)*SF(7) + P(12,12)*SF(14) - P(13,12)*SF(13) + P(1,12)*SPP(8) + P(3,12)*SPP(7) - (P(11,12)*q0)/2) + SPP(7)*(P(2,1) - P(4,1)*SF(7) + P(12,1)*SF(14) - P(13,1)*SF(13) + P(1,1)*SPP(8) + P(3,1)*SPP(7) - (P(11,1)*q0)/2) - SPP(8)*(P(2,3) - P(4,3)*SF(7) + P(12,3)*SF(14) - P(13,3)*SF(13) + P(1,3)*SPP(8) + P(3,3)*SPP(7) - (P(11,3)*q0)/2) - (q0*(P(2,13) - P(4,13)*SF(7) + P(12,13)*SF(14) - P(13,13)*SF(13) + P(1,13)*SPP(8) + P(3,13)*SPP(7) - (P(11,13)*q0)/2))/2;
nextP(3,4) = P(3,4) + SQ(4) + P(1,4)*SF(7) - P(11,4)*SF(14) + P(13,4)*SF(12) - P(2,4)*SPP(7) + P(4,4)*SPP(8) - (P(12,4)*q0)/2 + SF(7)*(P(3,2) + P(1,2)*SF(7) - P(11,2)*SF(14) + P(13,2)*SF(12) - P(2,2)*SPP(7) + P(4,2)*SPP(8) - (P(12,2)*q0)/2) + SF(13)*(P(3,11) + P(1,11)*SF(7) - P(11,11)*SF(14) + P(13,11)*SF(12) - P(2,11)*SPP(7) + P(4,11)*SPP(8) - (P(12,11)*q0)/2) - SF(12)*(P(3,12) + P(1,12)*SF(7) - P(11,12)*SF(14) + P(13,12)*SF(12) - P(2,12)*SPP(7) + P(4,12)*SPP(8) - (P(12,12)*q0)/2) + SPP(7)*(P(3,1) + P(1,1)*SF(7) - P(11,1)*SF(14) + P(13,1)*SF(12) - P(2,1)*SPP(7) + P(4,1)*SPP(8) - (P(12,1)*q0)/2) - SPP(8)*(P(3,3) + P(1,3)*SF(7) - P(11,3)*SF(14) + P(13,3)*SF(12) - P(2,3)*SPP(7) + P(4,3)*SPP(8) - (P(12,3)*q0)/2) - (q0*(P(3,13) + P(1,13)*SF(7) - P(11,13)*SF(14) + P(13,13)*SF(12) - P(2,13)*SPP(7) + P(4,13)*SPP(8) - (P(12,13)*q0)/2))/2;
nextP(4,4) = P(4,4) + P(2,4)*SF(7) + P(11,4)*SF(13) - P(12,4)*SF(12) + P(1,4)*SPP(7) - P(3,4)*SPP(8) + (dayCov*SQ(11))/4 + dazCov*SQ(10) - (P(13,4)*q0)/2 + SF(7)*(P(4,2) + P(2,2)*SF(7) + P(11,2)*SF(13) - P(12,2)*SF(12) + P(1,2)*SPP(7) - P(3,2)*SPP(8) - (P(13,2)*q0)/2) + SF(13)*(P(4,11) + P(2,11)*SF(7) + P(11,11)*SF(13) - P(12,11)*SF(12) + P(1,11)*SPP(7) - P(3,11)*SPP(8) - (P(13,11)*q0)/2) - SF(12)*(P(4,12) + P(2,12)*SF(7) + P(11,12)*SF(13) - P(12,12)*SF(12) + P(1,12)*SPP(7) - P(3,12)*SPP(8) - (P(13,12)*q0)/2) + SPP(7)*(P(4,1) + P(2,1)*SF(7) + P(11,1)*SF(13) - P(12,1)*SF(12) + P(1,1)*SPP(7) - P(3,1)*SPP(8) - (P(13,1)*q0)/2) - SPP(8)*(P(4,3) + P(2,3)*SF(7) + P(11,3)*SF(13) - P(12,3)*SF(12) + P(1,3)*SPP(7) - P(3,3)*SPP(8) - (P(13,3)*q0)/2) + (daxCov*q2^2)/4 - (q0*(P(4,13) + P(2,13)*SF(7) + P(11,13)*SF(13) - P(12,13)*SF(12) + P(1,13)*SPP(7) - P(3,13)*SPP(8) - (P(13,13)*q0)/2))/2;
nextP(1,5) = P(1,5) - P(3,5)*SF(7) + P(11,5)*SF(12) + P(12,5)*SF(13) + P(13,5)*SF(14) - P(2,5)*SPP(8) - P(4,5)*SPP(7) + SF(6)*(P(1,1) - P(3,1)*SF(7) + P(11,1)*SF(12) + P(12,1)*SF(13) + P(13,1)*SF(14) - P(2,1)*SPP(8) - P(4,1)*SPP(7)) + SF(4)*(P(1,2) - P(3,2)*SF(7) + P(11,2)*SF(12) + P(12,2)*SF(13) + P(13,2)*SF(14) - P(2,2)*SPP(8) - P(4,2)*SPP(7)) - SF(5)*(P(1,4) - P(3,4)*SF(7) + P(11,4)*SF(12) + P(12,4)*SF(13) + P(13,4)*SF(14) - P(2,4)*SPP(8) - P(4,4)*SPP(7)) + SPP(1)*(P(1,3) - P(3,3)*SF(7) + P(11,3)*SF(12) + P(12,3)*SF(13) + P(13,3)*SF(14) - P(2,3)*SPP(8) - P(4,3)*SPP(7)) - SPP(3)*(P(1,14) - P(3,14)*SF(7) + P(11,14)*SF(12) + P(12,14)*SF(13) + P(13,14)*SF(14) - P(2,14)*SPP(8) - P(4,14)*SPP(7)) + SPP(6)*(P(1,15) - P(3,15)*SF(7) + P(11,15)*SF(12) + P(12,15)*SF(13) + P(13,15)*SF(14) - P(2,15)*SPP(8) - P(4,15)*SPP(7)) - (2*q0*q2 + 2*q1*q3)*(P(1,16) - P(3,16)*SF(7) + P(11,16)*SF(12) + P(12,16)*SF(13) + P(13,16)*SF(14) - P(2,16)*SPP(8) - P(4,16)*SPP(7));
nextP(2,5) = P(2,5) - P(4,5)*SF(7) + P(12,5)*SF(14) - P(13,5)*SF(13) + P(1,5)*SPP(8) + P(3,5)*SPP(7) - (2*q0*q2 + 2*q1*q3)*(P(2,16) - P(4,16)*SF(7) + P(12,16)*SF(14) - P(13,16)*SF(13) + P(1,16)*SPP(8) + P(3,16)*SPP(7) - (P(11,16)*q0)/2) - (P(11,5)*q0)/2 + SF(6)*(P(2,1) - P(4,1)*SF(7) + P(12,1)*SF(14) - P(13,1)*SF(13) + P(1,1)*SPP(8) + P(3,1)*SPP(7) - (P(11,1)*q0)/2) + SF(4)*(P(2,2) - P(4,2)*SF(7) + P(12,2)*SF(14) - P(13,2)*SF(13) + P(1,2)*SPP(8) + P(3,2)*SPP(7) - (P(11,2)*q0)/2) - SF(5)*(P(2,4) - P(4,4)*SF(7) + P(12,4)*SF(14) - P(13,4)*SF(13) + P(1,4)*SPP(8) + P(3,4)*SPP(7) - (P(11,4)*q0)/2) + SPP(1)*(P(2,3) - P(4,3)*SF(7) + P(12,3)*SF(14) - P(13,3)*SF(13) + P(1,3)*SPP(8) + P(3,3)*SPP(7) - (P(11,3)*q0)/2) - SPP(3)*(P(2,14) - P(4,14)*SF(7) + P(12,14)*SF(14) - P(13,14)*SF(13) + P(1,14)*SPP(8) + P(3,14)*SPP(7) - (P(11,14)*q0)/2) + SPP(6)*(P(2,15) - P(4,15)*SF(7) + P(12,15)*SF(14) - P(13,15)*SF(13) + P(1,15)*SPP(8) + P(3,15)*SPP(7) - (P(11,15)*q0)/2);
nextP(3,5) = P(3,5) + P(1,5)*SF(7) - P(11,5)*SF(14) + P(13,5)*SF(12) - P(2,5)*SPP(7) + P(4,5)*SPP(8) - (2*q0*q2 + 2*q1*q3)*(P(3,16) + P(1,16)*SF(7) - P(11,16)*SF(14) + P(13,16)*SF(12) - P(2,16)*SPP(7) + P(4,16)*SPP(8) - (P(12,16)*q0)/2) - (P(12,5)*q0)/2 + SF(6)*(P(3,1) + P(1,1)*SF(7) - P(11,1)*SF(14) + P(13,1)*SF(12) - P(2,1)*SPP(7) + P(4,1)*SPP(8) - (P(12,1)*q0)/2) + SF(4)*(P(3,2) + P(1,2)*SF(7) - P(11,2)*SF(14) + P(13,2)*SF(12) - P(2,2)*SPP(7) + P(4,2)*SPP(8) - (P(12,2)*q0)/2) - SF(5)*(P(3,4) + P(1,4)*SF(7) - P(11,4)*SF(14) + P(13,4)*SF(12) - P(2,4)*SPP(7) + P(4,4)*SPP(8) - (P(12,4)*q0)/2) + SPP(1)*(P(3,3) + P(1,3)*SF(7) - P(11,3)*SF(14) + P(13,3)*SF(12) - P(2,3)*SPP(7) + P(4,3)*SPP(8) - (P(12,3)*q0)/2) - SPP(3)*(P(3,14) + P(1,14)*SF(7) - P(11,14)*SF(14) + P(13,14)*SF(12) - P(2,14)*SPP(7) + P(4,14)*SPP(8) - (P(12,14)*q0)/2) + SPP(6)*(P(3,15) + P(1,15)*SF(7) - P(11,15)*SF(14) + P(13,15)*SF(12) - P(2,15)*SPP(7) + P(4,15)*SPP(8) - (P(12,15)*q0)/2);
nextP(4,5) = P(4,5) + P(2,5)*SF(7) + P(11,5)*SF(13) - P(12,5)*SF(12) + P(1,5)*SPP(7) - P(3,5)*SPP(8) - (2*q0*q2 + 2*q1*q3)*(P(4,16) + P(2,16)*SF(7) + P(11,16)*SF(13) - P(12,16)*SF(12) + P(1,16)*SPP(7) - P(3,16)*SPP(8) - (P(13,16)*q0)/2) - (P(13,5)*q0)/2 + SF(6)*(P(4,1) + P(2,1)*SF(7) + P(11,1)*SF(13) - P(12,1)*SF(12) + P(1,1)*SPP(7) - P(3,1)*SPP(8) - (P(13,1)*q0)/2) + SF(4)*(P(4,2) + P(2,2)*SF(7) + P(11,2)*SF(13) - P(12,2)*SF(12) + P(1,2)*SPP(7) - P(3,2)*SPP(8) - (P(13,2)*q0)/2) - SF(5)*(P(4,4) + P(2,4)*SF(7) + P(11,4)*SF(13) - P(12,4)*SF(12) + P(1,4)*SPP(7) - P(3,4)*SPP(8) - (P(13,4)*q0)/2) + SPP(1)*(P(4,3) + P(2,3)*SF(7) + P(11,3)*SF(13) - P(12,3)*SF(12) + P(1,3)*SPP(7) - P(3,3)*SPP(8) - (P(13,3)*q0)/2) - SPP(3)*(P(4,14) + P(2,14)*SF(7) + P(11,14)*SF(13) - P(12,14)*SF(12) + P(1,14)*SPP(7) - P(3,14)*SPP(8) - (P(13,14)*q0)/2) + SPP(6)*(P(4,15) + P(2,15)*SF(7) + P(11,15)*SF(13) - P(12,15)*SF(12) + P(1,15)*SPP(7) - P(3,15)*SPP(8) - (P(13,15)*q0)/2);
nextP(5,5) = P(5,5) + P(1,5)*SF(6) + P(2,5)*SF(4) - P(4,5)*SF(5) + P(3,5)*SPP(1) - P(14,5)*SPP(3) + P(15,5)*SPP(6) + dvyCov*(SG(8) - 2*q0*q3)^2 + dvzCov*(SG(7) + 2*q0*q2)^2 - (2*q0*q2 + 2*q1*q3)*(P(5,16) + P(1,16)*SF(6) + P(2,16)*SF(4) - P(4,16)*SF(5) + P(3,16)*SPP(1) - P(14,16)*SPP(3) + P(15,16)*SPP(6) - P(16,16)*(2*q0*q2 + 2*q1*q3)) + dvxCov*(SG(2) + SG(3) - SG(4) - SG(5))^2 - P(16,5)*(2*q0*q2 + 2*q1*q3) + SF(6)*(P(5,1) + P(1,1)*SF(6) + P(2,1)*SF(4) - P(4,1)*SF(5) + P(3,1)*SPP(1) - P(14,1)*SPP(3) + P(15,1)*SPP(6) - P(16,1)*(2*q0*q2 + 2*q1*q3)) + SF(4)*(P(5,2) + P(1,2)*SF(6) + P(2,2)*SF(4) - P(4,2)*SF(5) + P(3,2)*SPP(1) - P(14,2)*SPP(3) + P(15,2)*SPP(6) - P(16,2)*(2*q0*q2 + 2*q1*q3)) - SF(5)*(P(5,4) + P(1,4)*SF(6) + P(2,4)*SF(4) - P(4,4)*SF(5) + P(3,4)*SPP(1) - P(14,4)*SPP(3) + P(15,4)*SPP(6) - P(16,4)*(2*q0*q2 + 2*q1*q3)) + SPP(1)*(P(5,3) + P(1,3)*SF(6) + P(2,3)*SF(4) - P(4,3)*SF(5) + P(3,3)*SPP(1) - P(14,3)*SPP(3) + P(15,3)*SPP(6) - P(16,3)*(2*q0*q2 + 2*q1*q3)) - SPP(3)*(P(5,14) + P(1,14)*SF(6) + P(2,14)*SF(4) - P(4,14)*SF(5) + P(3,14)*SPP(1) - P(14,14)*SPP(3) + P(15,14)*SPP(6) - P(16,14)*(2*q0*q2 + 2*q1*q3)) + SPP(6)*(P(5,15) + P(1,15)*SF(6) + P(2,15)*SF(4) - P(4,15)*SF(5) + P(3,15)*SPP(1) - P(14,15)*SPP(3) + P(15,15)*SPP(6) - P(16,15)*(2*q0*q2 + 2*q1*q3));
nextP(1,6) = P(1,6) - P(3,6)*SF(7) + P(11,6)*SF(12) + P(12,6)*SF(13) + P(13,6)*SF(14) - P(2,6)*SPP(8) - P(4,6)*SPP(7) + SF(5)*(P(1,1) - P(3,1)*SF(7) + P(11,1)*SF(12) + P(12,1)*SF(13) + P(13,1)*SF(14) - P(2,1)*SPP(8) - P(4,1)*SPP(7)) + SF(4)*(P(1,3) - P(3,3)*SF(7) + P(11,3)*SF(12) + P(12,3)*SF(13) + P(13,3)*SF(14) - P(2,3)*SPP(8) - P(4,3)*SPP(7)) + SF(6)*(P(1,4) - P(3,4)*SF(7) + P(11,4)*SF(12) + P(12,4)*SF(13) + P(13,4)*SF(14) - P(2,4)*SPP(8) - P(4,4)*SPP(7)) - SPP(1)*(P(1,2) - P(3,2)*SF(7) + P(11,2)*SF(12) + P(12,2)*SF(13) + P(13,2)*SF(14) - P(2,2)*SPP(8) - P(4,2)*SPP(7)) - SPP(2)*(P(1,15) - P(3,15)*SF(7) + P(11,15)*SF(12) + P(12,15)*SF(13) + P(13,15)*SF(14) - P(2,15)*SPP(8) - P(4,15)*SPP(7)) + SPP(5)*(P(1,16) - P(3,16)*SF(7) + P(11,16)*SF(12) + P(12,16)*SF(13) + P(13,16)*SF(14) - P(2,16)*SPP(8) - P(4,16)*SPP(7)) - (2*q0*q3 + 2*q1*q2)*(P(1,14) - P(3,14)*SF(7) + P(11,14)*SF(12) + P(12,14)*SF(13) + P(13,14)*SF(14) - P(2,14)*SPP(8) - P(4,14)*SPP(7));
nextP(2,6) = P(2,6) - P(4,6)*SF(7) + P(12,6)*SF(14) - P(13,6)*SF(13) + P(1,6)*SPP(8) + P(3,6)*SPP(7) - (2*q0*q3 + 2*q1*q2)*(P(2,14) - P(4,14)*SF(7) + P(12,14)*SF(14) - P(13,14)*SF(13) + P(1,14)*SPP(8) + P(3,14)*SPP(7) - (P(11,14)*q0)/2) - (P(11,6)*q0)/2 + SF(5)*(P(2,1) - P(4,1)*SF(7) + P(12,1)*SF(14) - P(13,1)*SF(13) + P(1,1)*SPP(8) + P(3,1)*SPP(7) - (P(11,1)*q0)/2) + SF(4)*(P(2,3) - P(4,3)*SF(7) + P(12,3)*SF(14) - P(13,3)*SF(13) + P(1,3)*SPP(8) + P(3,3)*SPP(7) - (P(11,3)*q0)/2) + SF(6)*(P(2,4) - P(4,4)*SF(7) + P(12,4)*SF(14) - P(13,4)*SF(13) + P(1,4)*SPP(8) + P(3,4)*SPP(7) - (P(11,4)*q0)/2) - SPP(1)*(P(2,2) - P(4,2)*SF(7) + P(12,2)*SF(14) - P(13,2)*SF(13) + P(1,2)*SPP(8) + P(3,2)*SPP(7) - (P(11,2)*q0)/2) - SPP(2)*(P(2,15) - P(4,15)*SF(7) + P(12,15)*SF(14) - P(13,15)*SF(13) + P(1,15)*SPP(8) + P(3,15)*SPP(7) - (P(11,15)*q0)/2) + SPP(5)*(P(2,16) - P(4,16)*SF(7) + P(12,16)*SF(14) - P(13,16)*SF(13) + P(1,16)*SPP(8) + P(3,16)*SPP(7) - (P(11,16)*q0)/2);
nextP(3,6) = P(3,6) + P(1,6)*SF(7) - P(11,6)*SF(14) + P(13,6)*SF(12) - P(2,6)*SPP(7) + P(4,6)*SPP(8) - (2*q0*q3 + 2*q1*q2)*(P(3,14) + P(1,14)*SF(7) - P(11,14)*SF(14) + P(13,14)*SF(12) - P(2,14)*SPP(7) + P(4,14)*SPP(8) - (P(12,14)*q0)/2) - (P(12,6)*q0)/2 + SF(5)*(P(3,1) + P(1,1)*SF(7) - P(11,1)*SF(14) + P(13,1)*SF(12) - P(2,1)*SPP(7) + P(4,1)*SPP(8) - (P(12,1)*q0)/2) + SF(4)*(P(3,3) + P(1,3)*SF(7) - P(11,3)*SF(14) + P(13,3)*SF(12) - P(2,3)*SPP(7) + P(4,3)*SPP(8) - (P(12,3)*q0)/2) + SF(6)*(P(3,4) + P(1,4)*SF(7) - P(11,4)*SF(14) + P(13,4)*SF(12) - P(2,4)*SPP(7) + P(4,4)*SPP(8) - (P(12,4)*q0)/2) - SPP(1)*(P(3,2) + P(1,2)*SF(7) - P(11,2)*SF(14) + P(13,2)*SF(12) - P(2,2)*SPP(7) + P(4,2)*SPP(8) - (P(12,2)*q0)/2) - SPP(2)*(P(3,15) + P(1,15)*SF(7) - P(11,15)*SF(14) + P(13,15)*SF(12) - P(2,15)*SPP(7) + P(4,15)*SPP(8) - (P(12,15)*q0)/2) + SPP(5)*(P(3,16) + P(1,16)*SF(7) - P(11,16)*SF(14) + P(13,16)*SF(12) - P(2,16)*SPP(7) + P(4,16)*SPP(8) - (P(12,16)*q0)/2);
nextP(4,6) = P(4,6) + P(2,6)*SF(7) + P(11,6)*SF(13) - P(12,6)*SF(12) + P(1,6)*SPP(7) - P(3,6)*SPP(8) - (2*q0*q3 + 2*q1*q2)*(P(4,14) + P(2,14)*SF(7) + P(11,14)*SF(13) - P(12,14)*SF(12) + P(1,14)*SPP(7) - P(3,14)*SPP(8) - (P(13,14)*q0)/2) - (P(13,6)*q0)/2 + SF(5)*(P(4,1) + P(2,1)*SF(7) + P(11,1)*SF(13) - P(12,1)*SF(12) + P(1,1)*SPP(7) - P(3,1)*SPP(8) - (P(13,1)*q0)/2) + SF(4)*(P(4,3) + P(2,3)*SF(7) + P(11,3)*SF(13) - P(12,3)*SF(12) + P(1,3)*SPP(7) - P(3,3)*SPP(8) - (P(13,3)*q0)/2) + SF(6)*(P(4,4) + P(2,4)*SF(7) + P(11,4)*SF(13) - P(12,4)*SF(12) + P(1,4)*SPP(7) - P(3,4)*SPP(8) - (P(13,4)*q0)/2) - SPP(1)*(P(4,2) + P(2,2)*SF(7) + P(11,2)*SF(13) - P(12,2)*SF(12) + P(1,2)*SPP(7) - P(3,2)*SPP(8) - (P(13,2)*q0)/2) - SPP(2)*(P(4,15) + P(2,15)*SF(7) + P(11,15)*SF(13) - P(12,15)*SF(12) + P(1,15)*SPP(7) - P(3,15)*SPP(8) - (P(13,15)*q0)/2) + SPP(5)*(P(4,16) + P(2,16)*SF(7) + P(11,16)*SF(13) - P(12,16)*SF(12) + P(1,16)*SPP(7) - P(3,16)*SPP(8) - (P(13,16)*q0)/2);
nextP(5,6) = P(5,6) + SQ(3) + P(1,6)*SF(6) + P(2,6)*SF(4) - P(4,6)*SF(5) + P(3,6)*SPP(1) - P(14,6)*SPP(3) + P(15,6)*SPP(6) - (2*q0*q3 + 2*q1*q2)*(P(5,14) + P(1,14)*SF(6) + P(2,14)*SF(4) - P(4,14)*SF(5) + P(3,14)*SPP(1) - P(14,14)*SPP(3) + P(15,14)*SPP(6) - P(16,14)*(2*q0*q2 + 2*q1*q3)) - P(16,6)*(2*q0*q2 + 2*q1*q3) + SF(5)*(P(5,1) + P(1,1)*SF(6) + P(2,1)*SF(4) - P(4,1)*SF(5) + P(3,1)*SPP(1) - P(14,1)*SPP(3) + P(15,1)*SPP(6) - P(16,1)*(2*q0*q2 + 2*q1*q3)) + SF(4)*(P(5,3) + P(1,3)*SF(6) + P(2,3)*SF(4) - P(4,3)*SF(5) + P(3,3)*SPP(1) - P(14,3)*SPP(3) + P(15,3)*SPP(6) - P(16,3)*(2*q0*q2 + 2*q1*q3)) + SF(6)*(P(5,4) + P(1,4)*SF(6) + P(2,4)*SF(4) - P(4,4)*SF(5) + P(3,4)*SPP(1) - P(14,4)*SPP(3) + P(15,4)*SPP(6) - P(16,4)*(2*q0*q2 + 2*q1*q3)) - SPP(1)*(P(5,2) + P(1,2)*SF(6) + P(2,2)*SF(4) - P(4,2)*SF(5) + P(3,2)*SPP(1) - P(14,2)*SPP(3) + P(15,2)*SPP(6) - P(16,2)*(2*q0*q2 + 2*q1*q3)) - SPP(2)*(P(5,15) + P(1,15)*SF(6) + P(2,15)*SF(4) - P(4,15)*SF(5) + P(3,15)*SPP(1) - P(14,15)*SPP(3) + P(15,15)*SPP(6) - P(16,15)*(2*q0*q2 + 2*q1*q3)) + SPP(5)*(P(5,16) + P(1,16)*SF(6) + P(2,16)*SF(4) - P(4,16)*SF(5) + P(3,16)*SPP(1) - P(14,16)*SPP(3) + P(15,16)*SPP(6) - P(16,16)*(2*q0*q2 + 2*q1*q3));
nextP(6,6) = P(6,6) + P(1,6)*SF(5) + P(3,6)*SF(4) + P(4,6)*SF(6) - P(2,6)*SPP(1) - P(15,6)*SPP(2) + P(16,6)*SPP(5) + dvxCov*(SG(8) + 2*q0*q3)^2 + dvzCov*(SG(6) - 2*q0*q1)^2 - (2*q0*q3 + 2*q1*q2)*(P(6,14) + P(1,14)*SF(5) + P(3,14)*SF(4) + P(4,14)*SF(6) - P(2,14)*SPP(1) - P(15,14)*SPP(2) + P(16,14)*SPP(5) - P(14,14)*(2*q0*q3 + 2*q1*q2)) + dvyCov*(SG(2) - SG(3) + SG(4) - SG(5))^2 - P(14,6)*(2*q0*q3 + 2*q1*q2) + SF(5)*(P(6,1) + P(1,1)*SF(5) + P(3,1)*SF(4) + P(4,1)*SF(6) - P(2,1)*SPP(1) - P(15,1)*SPP(2) + P(16,1)*SPP(5) - P(14,1)*(2*q0*q3 + 2*q1*q2)) + SF(4)*(P(6,3) + P(1,3)*SF(5) + P(3,3)*SF(4) + P(4,3)*SF(6) - P(2,3)*SPP(1) - P(15,3)*SPP(2) + P(16,3)*SPP(5) - P(14,3)*(2*q0*q3 + 2*q1*q2)) + SF(6)*(P(6,4) + P(1,4)*SF(5) + P(3,4)*SF(4) + P(4,4)*SF(6) - P(2,4)*SPP(1) - P(15,4)*SPP(2) + P(16,4)*SPP(5) - P(14,4)*(2*q0*q3 + 2*q1*q2)) - SPP(1)*(P(6,2) + P(1,2)*SF(5) + P(3,2)*SF(4) + P(4,2)*SF(6) - P(2,2)*SPP(1) - P(15,2)*SPP(2) + P(16,2)*SPP(5) - P(14,2)*(2*q0*q3 + 2*q1*q2)) - SPP(2)*(P(6,15) + P(1,15)*SF(5) + P(3,15)*SF(4) + P(4,15)*SF(6) - P(2,15)*SPP(1) - P(15,15)*SPP(2) + P(16,15)*SPP(5) - P(14,15)*(2*q0*q3 + 2*q1*q2)) + SPP(5)*(P(6,16) + P(1,16)*SF(5) + P(3,16)*SF(4) + P(4,16)*SF(6) - P(2,16)*SPP(1) - P(15,16)*SPP(2) + P(16,16)*SPP(5) - P(14,16)*(2*q0*q3 + 2*q1*q2));
nextP(1,7) = P(1,7) - P(3,7)*SF(7) + P(11,7)*SF(12) + P(12,7)*SF(13) + P(13,7)*SF(14) - P(2,7)*SPP(8) - P(4,7)*SPP(7) + SF(5)*(P(1,2) - P(3,2)*SF(7) + P(11,2)*SF(12) + P(12,2)*SF(13) + P(13,2)*SF(14) - P(2,2)*SPP(8) - P(4,2)*SPP(7)) - SF(6)*(P(1,3) - P(3,3)*SF(7) + P(11,3)*SF(12) + P(12,3)*SF(13) + P(13,3)*SF(14) - P(2,3)*SPP(8) - P(4,3)*SPP(7)) + SF(4)*(P(1,4) - P(3,4)*SF(7) + P(11,4)*SF(12) + P(12,4)*SF(13) + P(13,4)*SF(14) - P(2,4)*SPP(8) - P(4,4)*SPP(7)) + SPP(1)*(P(1,1) - P(3,1)*SF(7) + P(11,1)*SF(12) + P(12,1)*SF(13) + P(13,1)*SF(14) - P(2,1)*SPP(8) - P(4,1)*SPP(7)) + SPP(4)*(P(1,14) - P(3,14)*SF(7) + P(11,14)*SF(12) + P(12,14)*SF(13) + P(13,14)*SF(14) - P(2,14)*SPP(8) - P(4,14)*SPP(7)) - (q0^2 - q1^2 - q2^2 + q3^2)*(P(1,16) - P(3,16)*SF(7) + P(11,16)*SF(12) + P(12,16)*SF(13) + P(13,16)*SF(14) - P(2,16)*SPP(8) - P(4,16)*SPP(7)) - (2*q0*q1 + 2*q2*q3)*(P(1,15) - P(3,15)*SF(7) + P(11,15)*SF(12) + P(12,15)*SF(13) + P(13,15)*SF(14) - P(2,15)*SPP(8) - P(4,15)*SPP(7));
nextP(2,7) = P(2,7) - P(4,7)*SF(7) + P(12,7)*SF(14) - P(13,7)*SF(13) + P(1,7)*SPP(8) + P(3,7)*SPP(7) - (2*q0*q1 + 2*q2*q3)*(P(2,15) - P(4,15)*SF(7) + P(12,15)*SF(14) - P(13,15)*SF(13) + P(1,15)*SPP(8) + P(3,15)*SPP(7) - (P(11,15)*q0)/2) - (P(11,7)*q0)/2 + SF(5)*(P(2,2) - P(4,2)*SF(7) + P(12,2)*SF(14) - P(13,2)*SF(13) + P(1,2)*SPP(8) + P(3,2)*SPP(7) - (P(11,2)*q0)/2) - SF(6)*(P(2,3) - P(4,3)*SF(7) + P(12,3)*SF(14) - P(13,3)*SF(13) + P(1,3)*SPP(8) + P(3,3)*SPP(7) - (P(11,3)*q0)/2) + SF(4)*(P(2,4) - P(4,4)*SF(7) + P(12,4)*SF(14) - P(13,4)*SF(13) + P(1,4)*SPP(8) + P(3,4)*SPP(7) - (P(11,4)*q0)/2) + SPP(1)*(P(2,1) - P(4,1)*SF(7) + P(12,1)*SF(14) - P(13,1)*SF(13) + P(1,1)*SPP(8) + P(3,1)*SPP(7) - (P(11,1)*q0)/2) + SPP(4)*(P(2,14) - P(4,14)*SF(7) + P(12,14)*SF(14) - P(13,14)*SF(13) + P(1,14)*SPP(8) + P(3,14)*SPP(7) - (P(11,14)*q0)/2) - (q0^2 - q1^2 - q2^2 + q3^2)*(P(2,16) - P(4,16)*SF(7) + P(12,16)*SF(14) - P(13,16)*SF(13) + P(1,16)*SPP(8) + P(3,16)*SPP(7) - (P(11,16)*q0)/2);
nextP(3,7) = P(3,7) + P(1,7)*SF(7) - P(11,7)*SF(14) + P(13,7)*SF(12) - P(2,7)*SPP(7) + P(4,7)*SPP(8) - (2*q0*q1 + 2*q2*q3)*(P(3,15) + P(1,15)*SF(7) - P(11,15)*SF(14) + P(13,15)*SF(12) - P(2,15)*SPP(7) + P(4,15)*SPP(8) - (P(12,15)*q0)/2) - (P(12,7)*q0)/2 + SF(5)*(P(3,2) + P(1,2)*SF(7) - P(11,2)*SF(14) + P(13,2)*SF(12) - P(2,2)*SPP(7) + P(4,2)*SPP(8) - (P(12,2)*q0)/2) - SF(6)*(P(3,3) + P(1,3)*SF(7) - P(11,3)*SF(14) + P(13,3)*SF(12) - P(2,3)*SPP(7) + P(4,3)*SPP(8) - (P(12,3)*q0)/2) + SF(4)*(P(3,4) + P(1,4)*SF(7) - P(11,4)*SF(14) + P(13,4)*SF(12) - P(2,4)*SPP(7) + P(4,4)*SPP(8) - (P(12,4)*q0)/2) + SPP(1)*(P(3,1) + P(1,1)*SF(7) - P(11,1)*SF(14) + P(13,1)*SF(12) - P(2,1)*SPP(7) + P(4,1)*SPP(8) - (P(12,1)*q0)/2) + SPP(4)*(P(3,14) + P(1,14)*SF(7) - P(11,14)*SF(14) + P(13,14)*SF(12) - P(2,14)*SPP(7) + P(4,14)*SPP(8) - (P(12,14)*q0)/2) - (q0^2 - q1^2 - q2^2 + q3^2)*(P(3,16) + P(1,16)*SF(7) - P(11,16)*SF(14) + P(13,16)*SF(12) - P(2,16)*SPP(7) + P(4,16)*SPP(8) - (P(12,16)*q0)/2);
nextP(4,7) = P(4,7) + P(2,7)*SF(7) + P(11,7)*SF(13) - P(12,7)*SF(12) + P(1,7)*SPP(7) - P(3,7)*SPP(8) - (2*q0*q1 + 2*q2*q3)*(P(4,15) + P(2,15)*SF(7) + P(11,15)*SF(13) - P(12,15)*SF(12) + P(1,15)*SPP(7) - P(3,15)*SPP(8) - (P(13,15)*q0)/2) - (P(13,7)*q0)/2 + SF(5)*(P(4,2) + P(2,2)*SF(7) + P(11,2)*SF(13) - P(12,2)*SF(12) + P(1,2)*SPP(7) - P(3,2)*SPP(8) - (P(13,2)*q0)/2) - SF(6)*(P(4,3) + P(2,3)*SF(7) + P(11,3)*SF(13) - P(12,3)*SF(12) + P(1,3)*SPP(7) - P(3,3)*SPP(8) - (P(13,3)*q0)/2) + SF(4)*(P(4,4) + P(2,4)*SF(7) + P(11,4)*SF(13) - P(12,4)*SF(12) + P(1,4)*SPP(7) - P(3,4)*SPP(8) - (P(13,4)*q0)/2) + SPP(1)*(P(4,1) + P(2,1)*SF(7) + P(11,1)*SF(13) - P(12,1)*SF(12) + P(1,1)*SPP(7) - P(3,1)*SPP(8) - (P(13,1)*q0)/2) + SPP(4)*(P(4,14) + P(2,14)*SF(7) + P(11,14)*SF(13) - P(12,14)*SF(12) + P(1,14)*SPP(7) - P(3,14)*SPP(8) - (P(13,14)*q0)/2) - (q0^2 - q1^2 - q2^2 + q3^2)*(P(4,16) + P(2,16)*SF(7) + P(11,16)*SF(13) - P(12,16)*SF(12) + P(1,16)*SPP(7) - P(3,16)*SPP(8) - (P(13,16)*q0)/2);
nextP(5,7) = P(5,7) + SQ(2) + P(1,7)*SF(6) + P(2,7)*SF(4) - P(4,7)*SF(5) + P(3,7)*SPP(1) - P(14,7)*SPP(3) + P(15,7)*SPP(6) - (q0^2 - q1^2 - q2^2 + q3^2)*(P(5,16) + P(1,16)*SF(6) + P(2,16)*SF(4) - P(4,16)*SF(5) + P(3,16)*SPP(1) - P(14,16)*SPP(3) + P(15,16)*SPP(6) - P(16,16)*(2*q0*q2 + 2*q1*q3)) - (2*q0*q1 + 2*q2*q3)*(P(5,15) + P(1,15)*SF(6) + P(2,15)*SF(4) - P(4,15)*SF(5) + P(3,15)*SPP(1) - P(14,15)*SPP(3) + P(15,15)*SPP(6) - P(16,15)*(2*q0*q2 + 2*q1*q3)) - P(16,7)*(2*q0*q2 + 2*q1*q3) + SF(5)*(P(5,2) + P(1,2)*SF(6) + P(2,2)*SF(4) - P(4,2)*SF(5) + P(3,2)*SPP(1) - P(14,2)*SPP(3) + P(15,2)*SPP(6) - P(16,2)*(2*q0*q2 + 2*q1*q3)) - SF(6)*(P(5,3) + P(1,3)*SF(6) + P(2,3)*SF(4) - P(4,3)*SF(5) + P(3,3)*SPP(1) - P(14,3)*SPP(3) + P(15,3)*SPP(6) - P(16,3)*(2*q0*q2 + 2*q1*q3)) + SF(4)*(P(5,4) + P(1,4)*SF(6) + P(2,4)*SF(4) - P(4,4)*SF(5) + P(3,4)*SPP(1) - P(14,4)*SPP(3) + P(15,4)*SPP(6) - P(16,4)*(2*q0*q2 + 2*q1*q3)) + SPP(1)*(P(5,1) + P(1,1)*SF(6) + P(2,1)*SF(4) - P(4,1)*SF(5) + P(3,1)*SPP(1) - P(14,1)*SPP(3) + P(15,1)*SPP(6) - P(16,1)*(2*q0*q2 + 2*q1*q3)) + SPP(4)*(P(5,14) + P(1,14)*SF(6) + P(2,14)*SF(4) - P(4,14)*SF(5) + P(3,14)*SPP(1) - P(14,14)*SPP(3) + P(15,14)*SPP(6) - P(16,14)*(2*q0*q2 + 2*q1*q3));
nextP(6,7) = P(6,7) + SQ(1) + P(1,7)*SF(5) + P(3,7)*SF(4) + P(4,7)*SF(6) - P(2,7)*SPP(1) - P(15,7)*SPP(2) + P(16,7)*SPP(5) - (q0^2 - q1^2 - q2^2 + q3^2)*(P(6,16) + P(1,16)*SF(5) + P(3,16)*SF(4) + P(4,16)*SF(6) - P(2,16)*SPP(1) - P(15,16)*SPP(2) + P(16,16)*SPP(5) - P(14,16)*(2*q0*q3 + 2*q1*q2)) - (2*q0*q1 + 2*q2*q3)*(P(6,15) + P(1,15)*SF(5) + P(3,15)*SF(4) + P(4,15)*SF(6) - P(2,15)*SPP(1) - P(15,15)*SPP(2) + P(16,15)*SPP(5) - P(14,15)*(2*q0*q3 + 2*q1*q2)) - P(14,7)*(2*q0*q3 + 2*q1*q2) + SF(5)*(P(6,2) + P(1,2)*SF(5) + P(3,2)*SF(4) + P(4,2)*SF(6) - P(2,2)*SPP(1) - P(15,2)*SPP(2) + P(16,2)*SPP(5) - P(14,2)*(2*q0*q3 + 2*q1*q2)) - SF(6)*(P(6,3) + P(1,3)*SF(5) + P(3,3)*SF(4) + P(4,3)*SF(6) - P(2,3)*SPP(1) - P(15,3)*SPP(2) + P(16,3)*SPP(5) - P(14,3)*(2*q0*q3 + 2*q1*q2)) + SF(4)*(P(6,4) + P(1,4)*SF(5) + P(3,4)*SF(4) + P(4,4)*SF(6) - P(2,4)*SPP(1) - P(15,4)*SPP(2) + P(16,4)*SPP(5) - P(14,4)*(2*q0*q3 + 2*q1*q2)) + SPP(1)*(P(6,1) + P(1,1)*SF(5) + P(3,1)*SF(4) + P(4,1)*SF(6) - P(2,1)*SPP(1) - P(15,1)*SPP(2) + P(16,1)*SPP(5) - P(14,1)*(2*q0*q3 + 2*q1*q2)) + SPP(4)*(P(6,14) + P(1,14)*SF(5) + P(3,14)*SF(4) + P(4,14)*SF(6) - P(2,14)*SPP(1) - P(15,14)*SPP(2) + P(16,14)*SPP(5) - P(14,14)*(2*q0*q3 + 2*q1*q2));
nextP(7,7) = P(7,7) + P(2,7)*SF(5) - P(3,7)*SF(6) + P(4,7)*SF(4) + P(1,7)*SPP(1) + P(14,7)*SPP(4) + SF(5)*(P(7,2) + P(2,2)*SF(5) - P(3,2)*SF(6) + P(4,2)*SF(4) + P(1,2)*SPP(1) + P(14,2)*SPP(4) - P(16,2)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,2)*(2*q0*q1 + 2*q2*q3)) - SF(6)*(P(7,3) + P(2,3)*SF(5) - P(3,3)*SF(6) + P(4,3)*SF(4) + P(1,3)*SPP(1) + P(14,3)*SPP(4) - P(16,3)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,3)*(2*q0*q1 + 2*q2*q3)) + SF(4)*(P(7,4) + P(2,4)*SF(5) - P(3,4)*SF(6) + P(4,4)*SF(4) + P(1,4)*SPP(1) + P(14,4)*SPP(4) - P(16,4)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,4)*(2*q0*q1 + 2*q2*q3)) + SPP(1)*(P(7,1) + P(2,1)*SF(5) - P(3,1)*SF(6) + P(4,1)*SF(4) + P(1,1)*SPP(1) + P(14,1)*SPP(4) - P(16,1)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,1)*(2*q0*q1 + 2*q2*q3)) + SPP(4)*(P(7,14) + P(2,14)*SF(5) - P(3,14)*SF(6) + P(4,14)*SF(4) + P(1,14)*SPP(1) + P(14,14)*SPP(4) - P(16,14)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,14)*(2*q0*q1 + 2*q2*q3)) - P(16,7)*(q0^2 - q1^2 - q2^2 + q3^2) + dvxCov*(SG(7) - 2*q0*q2)^2 + dvyCov*(SG(6) + 2*q0*q1)^2 - (q0^2 - q1^2 - q2^2 + q3^2)*(P(7,16) + P(2,16)*SF(5) - P(3,16)*SF(6) + P(4,16)*SF(4) + P(1,16)*SPP(1) + P(14,16)*SPP(4) - P(16,16)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,16)*(2*q0*q1 + 2*q2*q3)) + dvzCov*(SG(2) - SG(3) - SG(4) + SG(5))^2 - P(15,7)*(2*q0*q1 + 2*q2*q3) - (2*q0*q1 + 2*q2*q3)*(P(7,15) + P(2,15)*SF(5) - P(3,15)*SF(6) + P(4,15)*SF(4) + P(1,15)*SPP(1) + P(14,15)*SPP(4) - P(16,15)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,15)*(2*q0*q1 + 2*q2*q3));
nextP(1,8) = P(1,8) - P(3,8)*SF(7) + P(11,8)*SF(12) + P(12,8)*SF(13) + P(13,8)*SF(14) - P(2,8)*SPP(8) - P(4,8)*SPP(7) + dt*(P(1,5) - P(3,5)*SF(7) + P(11,5)*SF(12) + P(12,5)*SF(13) + P(13,5)*SF(14) - P(2,5)*SPP(8) - P(4,5)*SPP(7));
nextP(2,8) = P(2,8) - P(4,8)*SF(7) + P(12,8)*SF(14) - P(13,8)*SF(13) + P(1,8)*SPP(8) + P(3,8)*SPP(7) - (P(11,8)*q0)/2 + dt*(P(2,5) - P(4,5)*SF(7) + P(12,5)*SF(14) - P(13,5)*SF(13) + P(1,5)*SPP(8) + P(3,5)*SPP(7) - (P(11,5)*q0)/2);
nextP(3,8) = P(3,8) + P(1,8)*SF(7) - P(11,8)*SF(14) + P(13,8)*SF(12) - P(2,8)*SPP(7) + P(4,8)*SPP(8) - (P(12,8)*q0)/2 + dt*(P(3,5) + P(1,5)*SF(7) - P(11,5)*SF(14) + P(13,5)*SF(12) - P(2,5)*SPP(7) + P(4,5)*SPP(8) - (P(12,5)*q0)/2);
nextP(4,8) = P(4,8) + P(2,8)*SF(7) + P(11,8)*SF(13) - P(12,8)*SF(12) + P(1,8)*SPP(7) - P(3,8)*SPP(8) - (P(13,8)*q0)/2 + dt*(P(4,5) + P(2,5)*SF(7) + P(11,5)*SF(13) - P(12,5)*SF(12) + P(1,5)*SPP(7) - P(3,5)*SPP(8) - (P(13,5)*q0)/2);
nextP(5,8) = P(5,8) + P(1,8)*SF(6) + P(2,8)*SF(4) - P(4,8)*SF(5) + P(3,8)*SPP(1) - P(14,8)*SPP(3) + P(15,8)*SPP(6) + dt*(P(5,5) + P(1,5)*SF(6) + P(2,5)*SF(4) - P(4,5)*SF(5) + P(3,5)*SPP(1) - P(14,5)*SPP(3) + P(15,5)*SPP(6) - P(16,5)*(2*q0*q2 + 2*q1*q3)) - P(16,8)*(2*q0*q2 + 2*q1*q3);
nextP(6,8) = P(6,8) + P(1,8)*SF(5) + P(3,8)*SF(4) + P(4,8)*SF(6) - P(2,8)*SPP(1) - P(15,8)*SPP(2) + P(16,8)*SPP(5) + dt*(P(6,5) + P(1,5)*SF(5) + P(3,5)*SF(4) + P(4,5)*SF(6) - P(2,5)*SPP(1) - P(15,5)*SPP(2) + P(16,5)*SPP(5) - P(14,5)*(2*q0*q3 + 2*q1*q2)) - P(14,8)*(2*q0*q3 + 2*q1*q2);
nextP(7,8) = P(7,8) + P(2,8)*SF(5) - P(3,8)*SF(6) + P(4,8)*SF(4) + P(1,8)*SPP(1) + P(14,8)*SPP(4) - P(16,8)*(q0^2 - q1^2 - q2^2 + q3^2) + dt*(P(7,5) + P(2,5)*SF(5) - P(3,5)*SF(6) + P(4,5)*SF(4) + P(1,5)*SPP(1) + P(14,5)*SPP(4) - P(16,5)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,5)*(2*q0*q1 + 2*q2*q3)) - P(15,8)*(2*q0*q1 + 2*q2*q3);
nextP(8,8) = P(8,8) + P(5,8)*dt + dt*(P(8,5) + P(5,5)*dt);
nextP(1,9) = P(1,9) - P(3,9)*SF(7) + P(11,9)*SF(12) + P(12,9)*SF(13) + P(13,9)*SF(14) - P(2,9)*SPP(8) - P(4,9)*SPP(7) + dt*(P(1,6) - P(3,6)*SF(7) + P(11,6)*SF(12) + P(12,6)*SF(13) + P(13,6)*SF(14) - P(2,6)*SPP(8) - P(4,6)*SPP(7));
nextP(2,9) = P(2,9) - P(4,9)*SF(7) + P(12,9)*SF(14) - P(13,9)*SF(13) + P(1,9)*SPP(8) + P(3,9)*SPP(7) - (P(11,9)*q0)/2 + dt*(P(2,6) - P(4,6)*SF(7) + P(12,6)*SF(14) - P(13,6)*SF(13) + P(1,6)*SPP(8) + P(3,6)*SPP(7) - (P(11,6)*q0)/2);
nextP(3,9) = P(3,9) + P(1,9)*SF(7) - P(11,9)*SF(14) + P(13,9)*SF(12) - P(2,9)*SPP(7) + P(4,9)*SPP(8) - (P(12,9)*q0)/2 + dt*(P(3,6) + P(1,6)*SF(7) - P(11,6)*SF(14) + P(13,6)*SF(12) - P(2,6)*SPP(7) + P(4,6)*SPP(8) - (P(12,6)*q0)/2);
nextP(4,9) = P(4,9) + P(2,9)*SF(7) + P(11,9)*SF(13) - P(12,9)*SF(12) + P(1,9)*SPP(7) - P(3,9)*SPP(8) - (P(13,9)*q0)/2 + dt*(P(4,6) + P(2,6)*SF(7) + P(11,6)*SF(13) - P(12,6)*SF(12) + P(1,6)*SPP(7) - P(3,6)*SPP(8) - (P(13,6)*q0)/2);
nextP(5,9) = P(5,9) + P(1,9)*SF(6) + P(2,9)*SF(4) - P(4,9)*SF(5) + P(3,9)*SPP(1) - P(14,9)*SPP(3) + P(15,9)*SPP(6) + dt*(P(5,6) + P(1,6)*SF(6) + P(2,6)*SF(4) - P(4,6)*SF(5) + P(3,6)*SPP(1) - P(14,6)*SPP(3) + P(15,6)*SPP(6) - P(16,6)*(2*q0*q2 + 2*q1*q3)) - P(16,9)*(2*q0*q2 + 2*q1*q3);
nextP(6,9) = P(6,9) + P(1,9)*SF(5) + P(3,9)*SF(4) + P(4,9)*SF(6) - P(2,9)*SPP(1) - P(15,9)*SPP(2) + P(16,9)*SPP(5) + dt*(P(6,6) + P(1,6)*SF(5) + P(3,6)*SF(4) + P(4,6)*SF(6) - P(2,6)*SPP(1) - P(15,6)*SPP(2) + P(16,6)*SPP(5) - P(14,6)*(2*q0*q3 + 2*q1*q2)) - P(14,9)*(2*q0*q3 + 2*q1*q2);
nextP(7,9) = P(7,9) + P(2,9)*SF(5) - P(3,9)*SF(6) + P(4,9)*SF(4) + P(1,9)*SPP(1) + P(14,9)*SPP(4) - P(16,9)*(q0^2 - q1^2 - q2^2 + q3^2) + dt*(P(7,6) + P(2,6)*SF(5) - P(3,6)*SF(6) + P(4,6)*SF(4) + P(1,6)*SPP(1) + P(14,6)*SPP(4) - P(16,6)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,6)*(2*q0*q1 + 2*q2*q3)) - P(15,9)*(2*q0*q1 + 2*q2*q3);
nextP(8,9) = P(8,9) + P(5,9)*dt + dt*(P(8,6) + P(5,6)*dt);
nextP(9,9) = P(9,9) + P(6,9)*dt + dt*(P(9,6) + P(6,6)*dt);
nextP(1,10) = P(1,10) - P(3,10)*SF(7) + P(11,10)*SF(12) + P(12,10)*SF(13) + P(13,10)*SF(14) - P(2,10)*SPP(8) - P(4,10)*SPP(7) + dt*(P(1,7) - P(3,7)*SF(7) + P(11,7)*SF(12) + P(12,7)*SF(13) + P(13,7)*SF(14) - P(2,7)*SPP(8) - P(4,7)*SPP(7));
nextP(2,10) = P(2,10) - P(4,10)*SF(7) + P(12,10)*SF(14) - P(13,10)*SF(13) + P(1,10)*SPP(8) + P(3,10)*SPP(7) - (P(11,10)*q0)/2 + dt*(P(2,7) - P(4,7)*SF(7) + P(12,7)*SF(14) - P(13,7)*SF(13) + P(1,7)*SPP(8) + P(3,7)*SPP(7) - (P(11,7)*q0)/2);
nextP(3,10) = P(3,10) + P(1,10)*SF(7) - P(11,10)*SF(14) + P(13,10)*SF(12) - P(2,10)*SPP(7) + P(4,10)*SPP(8) - (P(12,10)*q0)/2 + dt*(P(3,7) + P(1,7)*SF(7) - P(11,7)*SF(14) + P(13,7)*SF(12) - P(2,7)*SPP(7) + P(4,7)*SPP(8) - (P(12,7)*q0)/2);
nextP(4,10) = P(4,10) + P(2,10)*SF(7) + P(11,10)*SF(13) - P(12,10)*SF(12) + P(1,10)*SPP(7) - P(3,10)*SPP(8) - (P(13,10)*q0)/2 + dt*(P(4,7) + P(2,7)*SF(7) + P(11,7)*SF(13) - P(12,7)*SF(12) + P(1,7)*SPP(7) - P(3,7)*SPP(8) - (P(13,7)*q0)/2);
nextP(5,10) = P(5,10) + P(1,10)*SF(6) + P(2,10)*SF(4) - P(4,10)*SF(5) + P(3,10)*SPP(1) - P(14,10)*SPP(3) + P(15,10)*SPP(6) + dt*(P(5,7) + P(1,7)*SF(6) + P(2,7)*SF(4) - P(4,7)*SF(5) + P(3,7)*SPP(1) - P(14,7)*SPP(3) + P(15,7)*SPP(6) - P(16,7)*(2*q0*q2 + 2*q1*q3)) - P(16,10)*(2*q0*q2 + 2*q1*q3);
nextP(6,10) = P(6,10) + P(1,10)*SF(5) + P(3,10)*SF(4) + P(4,10)*SF(6) - P(2,10)*SPP(1) - P(15,10)*SPP(2) + P(16,10)*SPP(5) + dt*(P(6,7) + P(1,7)*SF(5) + P(3,7)*SF(4) + P(4,7)*SF(6) - P(2,7)*SPP(1) - P(15,7)*SPP(2) + P(16,7)*SPP(5) - P(14,7)*(2*q0*q3 + 2*q1*q2)) - P(14,10)*(2*q0*q3 + 2*q1*q2);
nextP(7,10) = P(7,10) + P(2,10)*SF(5) - P(3,10)*SF(6) + P(4,10)*SF(4) + P(1,10)*SPP(1) + P(14,10)*SPP(4) - P(16,10)*(q0^2 - q1^2 - q2^2 + q3^2) + dt*(P(7,7) + P(2,7)*SF(5) - P(3,7)*SF(6) + P(4,7)*SF(4) + P(1,7)*SPP(1) + P(14,7)*SPP(4) - P(16,7)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,7)*(2*q0*q1 + 2*q2*q3)) - P(15,10)*(2*q0*q1 + 2*q2*q3);
nextP(8,10) = P(8,10) + P(5,10)*dt + dt*(P(8,7) + P(5,7)*dt);
nextP(9,10) = P(9,10) + P(6,10)*dt + dt*(P(9,7) + P(6,7)*dt);
nextP(10,10) = P(10,10) + P(7,10)*dt + dt*(P(10,7) + P(7,7)*dt);
nextP(1,11) = P(1,11) - P(3,11)*SF(7) + P(11,11)*SF(12) + P(12,11)*SF(13) + P(13,11)*SF(14) - P(2,11)*SPP(8) - P(4,11)*SPP(7);
nextP(2,11) = P(2,11) - P(4,11)*SF(7) + P(12,11)*SF(14) - P(13,11)*SF(13) + P(1,11)*SPP(8) + P(3,11)*SPP(7) - (P(11,11)*q0)/2;
nextP(3,11) = P(3,11) + P(1,11)*SF(7) - P(11,11)*SF(14) + P(13,11)*SF(12) - P(2,11)*SPP(7) + P(4,11)*SPP(8) - (P(12,11)*q0)/2;
nextP(4,11) = P(4,11) + P(2,11)*SF(7) + P(11,11)*SF(13) - P(12,11)*SF(12) + P(1,11)*SPP(7) - P(3,11)*SPP(8) - (P(13,11)*q0)/2;
nextP(5,11) = P(5,11) + P(1,11)*SF(6) + P(2,11)*SF(4) - P(4,11)*SF(5) + P(3,11)*SPP(1) - P(14,11)*SPP(3) + P(15,11)*SPP(6) - P(16,11)*(2*q0*q2 + 2*q1*q3);
nextP(6,11) = P(6,11) + P(1,11)*SF(5) + P(3,11)*SF(4) + P(4,11)*SF(6) - P(2,11)*SPP(1) - P(15,11)*SPP(2) + P(16,11)*SPP(5) - P(14,11)*(2*q0*q3 + 2*q1*q2);
nextP(7,11) = P(7,11) + P(2,11)*SF(5) - P(3,11)*SF(6) + P(4,11)*SF(4) + P(1,11)*SPP(1) + P(14,11)*SPP(4) - P(16,11)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,11)*(2*q0*q1 + 2*q2*q3);
nextP(8,11) = P(8,11) + P(5,11)*dt;
nextP(9,11) = P(9,11) + P(6,11)*dt;
nextP(10,11) = P(10,11) + P(7,11)*dt;
nextP(11,11) = P(11,11);
nextP(1,12) = P(1,12) - P(3,12)*SF(7) + P(11,12)*SF(12) + P(12,12)*SF(13) + P(13,12)*SF(14) - P(2,12)*SPP(8) - P(4,12)*SPP(7);
nextP(2,12) = P(2,12) - P(4,12)*SF(7) + P(12,12)*SF(14) - P(13,12)*SF(13) + P(1,12)*SPP(8) + P(3,12)*SPP(7) - (P(11,12)*q0)/2;
nextP(3,12) = P(3,12) + P(1,12)*SF(7) - P(11,12)*SF(14) + P(13,12)*SF(12) - P(2,12)*SPP(7) + P(4,12)*SPP(8) - (P(12,12)*q0)/2;
nextP(4,12) = P(4,12) + P(2,12)*SF(7) + P(11,12)*SF(13) - P(12,12)*SF(12) + P(1,12)*SPP(7) - P(3,12)*SPP(8) - (P(13,12)*q0)/2;
nextP(5,12) = P(5,12) + P(1,12)*SF(6) + P(2,12)*SF(4) - P(4,12)*SF(5) + P(3,12)*SPP(1) - P(14,12)*SPP(3) + P(15,12)*SPP(6) - P(16,12)*(2*q0*q2 + 2*q1*q3);
nextP(6,12) = P(6,12) + P(1,12)*SF(5) + P(3,12)*SF(4) + P(4,12)*SF(6) - P(2,12)*SPP(1) - P(15,12)*SPP(2) + P(16,12)*SPP(5) - P(14,12)*(2*q0*q3 + 2*q1*q2);
nextP(7,12) = P(7,12) + P(2,12)*SF(5) - P(3,12)*SF(6) + P(4,12)*SF(4) + P(1,12)*SPP(1) + P(14,12)*SPP(4) - P(16,12)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,12)*(2*q0*q1 + 2*q2*q3);
nextP(8,12) = P(8,12) + P(5,12)*dt;
nextP(9,12) = P(9,12) + P(6,12)*dt;
nextP(10,12) = P(10,12) + P(7,12)*dt;
nextP(11,12) = P(11,12);
nextP(12,12) = P(12,12);
nextP(1,13) = P(1,13) - P(3,13)*SF(7) + P(11,13)*SF(12) + P(12,13)*SF(13) + P(13,13)*SF(14) - P(2,13)*SPP(8) - P(4,13)*SPP(7);
nextP(2,13) = P(2,13) - P(4,13)*SF(7) + P(12,13)*SF(14) - P(13,13)*SF(13) + P(1,13)*SPP(8) + P(3,13)*SPP(7) - (P(11,13)*q0)/2;
nextP(3,13) = P(3,13) + P(1,13)*SF(7) - P(11,13)*SF(14) + P(13,13)*SF(12) - P(2,13)*SPP(7) + P(4,13)*SPP(8) - (P(12,13)*q0)/2;
nextP(4,13) = P(4,13) + P(2,13)*SF(7) + P(11,13)*SF(13) - P(12,13)*SF(12) + P(1,13)*SPP(7) - P(3,13)*SPP(8) - (P(13,13)*q0)/2;
nextP(5,13) = P(5,13) + P(1,13)*SF(6) + P(2,13)*SF(4) - P(4,13)*SF(5) + P(3,13)*SPP(1) - P(14,13)*SPP(3) + P(15,13)*SPP(6) - P(16,13)*(2*q0*q2 + 2*q1*q3);
nextP(6,13) = P(6,13) + P(1,13)*SF(5) + P(3,13)*SF(4) + P(4,13)*SF(6) - P(2,13)*SPP(1) - P(15,13)*SPP(2) + P(16,13)*SPP(5) - P(14,13)*(2*q0*q3 + 2*q1*q2);
nextP(7,13) = P(7,13) + P(2,13)*SF(5) - P(3,13)*SF(6) + P(4,13)*SF(4) + P(1,13)*SPP(1) + P(14,13)*SPP(4) - P(16,13)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,13)*(2*q0*q1 + 2*q2*q3);
nextP(8,13) = P(8,13) + P(5,13)*dt;
nextP(9,13) = P(9,13) + P(6,13)*dt;
nextP(10,13) = P(10,13) + P(7,13)*dt;
nextP(11,13) = P(11,13);
nextP(12,13) = P(12,13);
nextP(13,13) = P(13,13);
nextP(1,14) = P(1,14) - P(3,14)*SF(7) + P(11,14)*SF(12) + P(12,14)*SF(13) + P(13,14)*SF(14) - P(2,14)*SPP(8) - P(4,14)*SPP(7);
nextP(2,14) = P(2,14) - P(4,14)*SF(7) + P(12,14)*SF(14) - P(13,14)*SF(13) + P(1,14)*SPP(8) + P(3,14)*SPP(7) - (P(11,14)*q0)/2;
nextP(3,14) = P(3,14) + P(1,14)*SF(7) - P(11,14)*SF(14) + P(13,14)*SF(12) - P(2,14)*SPP(7) + P(4,14)*SPP(8) - (P(12,14)*q0)/2;
nextP(4,14) = P(4,14) + P(2,14)*SF(7) + P(11,14)*SF(13) - P(12,14)*SF(12) + P(1,14)*SPP(7) - P(3,14)*SPP(8) - (P(13,14)*q0)/2;
nextP(5,14) = P(5,14) + P(1,14)*SF(6) + P(2,14)*SF(4) - P(4,14)*SF(5) + P(3,14)*SPP(1) - P(14,14)*SPP(3) + P(15,14)*SPP(6) - P(16,14)*(2*q0*q2 + 2*q1*q3);
nextP(6,14) = P(6,14) + P(1,14)*SF(5) + P(3,14)*SF(4) + P(4,14)*SF(6) - P(2,14)*SPP(1) - P(15,14)*SPP(2) + P(16,14)*SPP(5) - P(14,14)*(2*q0*q3 + 2*q1*q2);
nextP(7,14) = P(7,14) + P(2,14)*SF(5) - P(3,14)*SF(6) + P(4,14)*SF(4) + P(1,14)*SPP(1) + P(14,14)*SPP(4) - P(16,14)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,14)*(2*q0*q1 + 2*q2*q3);
nextP(8,14) = P(8,14) + P(5,14)*dt;
nextP(9,14) = P(9,14) + P(6,14)*dt;
nextP(10,14) = P(10,14) + P(7,14)*dt;
nextP(11,14) = P(11,14);
nextP(12,14) = P(12,14);
nextP(13,14) = P(13,14);
nextP(14,14) = P(14,14);
nextP(1,15) = P(1,15) - P(3,15)*SF(7) + P(11,15)*SF(12) + P(12,15)*SF(13) + P(13,15)*SF(14) - P(2,15)*SPP(8) - P(4,15)*SPP(7);
nextP(2,15) = P(2,15) - P(4,15)*SF(7) + P(12,15)*SF(14) - P(13,15)*SF(13) + P(1,15)*SPP(8) + P(3,15)*SPP(7) - (P(11,15)*q0)/2;
nextP(3,15) = P(3,15) + P(1,15)*SF(7) - P(11,15)*SF(14) + P(13,15)*SF(12) - P(2,15)*SPP(7) + P(4,15)*SPP(8) - (P(12,15)*q0)/2;
nextP(4,15) = P(4,15) + P(2,15)*SF(7) + P(11,15)*SF(13) - P(12,15)*SF(12) + P(1,15)*SPP(7) - P(3,15)*SPP(8) - (P(13,15)*q0)/2;
nextP(5,15) = P(5,15) + P(1,15)*SF(6) + P(2,15)*SF(4) - P(4,15)*SF(5) + P(3,15)*SPP(1) - P(14,15)*SPP(3) + P(15,15)*SPP(6) - P(16,15)*(2*q0*q2 + 2*q1*q3);
nextP(6,15) = P(6,15) + P(1,15)*SF(5) + P(3,15)*SF(4) + P(4,15)*SF(6) - P(2,15)*SPP(1) - P(15,15)*SPP(2) + P(16,15)*SPP(5) - P(14,15)*(2*q0*q3 + 2*q1*q2);
nextP(7,15) = P(7,15) + P(2,15)*SF(5) - P(3,15)*SF(6) + P(4,15)*SF(4) + P(1,15)*SPP(1) + P(14,15)*SPP(4) - P(16,15)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,15)*(2*q0*q1 + 2*q2*q3);
nextP(8,15) = P(8,15) + P(5,15)*dt;
nextP(9,15) = P(9,15) + P(6,15)*dt;
nextP(10,15) = P(10,15) + P(7,15)*dt;
nextP(11,15) = P(11,15);
nextP(12,15) = P(12,15);
nextP(13,15) = P(13,15);
nextP(14,15) = P(14,15);
nextP(15,15) = P(15,15);
nextP(1,16) = P(1,16) - P(3,16)*SF(7) + P(11,16)*SF(12) + P(12,16)*SF(13) + P(13,16)*SF(14) - P(2,16)*SPP(8) - P(4,16)*SPP(7);
nextP(2,16) = P(2,16) - P(4,16)*SF(7) + P(12,16)*SF(14) - P(13,16)*SF(13) + P(1,16)*SPP(8) + P(3,16)*SPP(7) - (P(11,16)*q0)/2;
nextP(3,16) = P(3,16) + P(1,16)*SF(7) - P(11,16)*SF(14) + P(13,16)*SF(12) - P(2,16)*SPP(7) + P(4,16)*SPP(8) - (P(12,16)*q0)/2;
nextP(4,16) = P(4,16) + P(2,16)*SF(7) + P(11,16)*SF(13) - P(12,16)*SF(12) + P(1,16)*SPP(7) - P(3,16)*SPP(8) - (P(13,16)*q0)/2;
nextP(5,16) = P(5,16) + P(1,16)*SF(6) + P(2,16)*SF(4) - P(4,16)*SF(5) + P(3,16)*SPP(1) - P(14,16)*SPP(3) + P(15,16)*SPP(6) - P(16,16)*(2*q0*q2 + 2*q1*q3);
nextP(6,16) = P(6,16) + P(1,16)*SF(5) + P(3,16)*SF(4) + P(4,16)*SF(6) - P(2,16)*SPP(1) - P(15,16)*SPP(2) + P(16,16)*SPP(5) - P(14,16)*(2*q0*q3 + 2*q1*q2);
nextP(7,16) = P(7,16) + P(2,16)*SF(5) - P(3,16)*SF(6) + P(4,16)*SF(4) + P(1,16)*SPP(1) + P(14,16)*SPP(4) - P(16,16)*(q0^2 - q1^2 - q2^2 + q3^2) - P(15,16)*(2*q0*q1 + 2*q2*q3);
nextP(8,16) = P(8,16) + P(5,16)*dt;
nextP(9,16) = P(9,16) + P(6,16)*dt;
nextP(10,16) = P(10,16) + P(7,16)*dt;
nextP(11,16) = P(11,16);
nextP(12,16) = P(12,16);
nextP(13,16) = P(13,16);
nextP(14,16) = P(14,16);
nextP(15,16) = P(15,16);
nextP(16,16) = P(16,16);
    
    % Add the process noise
    for i = 1:16
        nextP(i,i) = nextP(i,i) + processNoise(i);
    end
    
    % If the total position variance exceds 1E6 (1000m), then stop covariance
    % growth by setting the predicted to the previous values
    % This prevent an ill conditioned matrix from occurring for long periods
    % without GPS
    if (P(8,8) + P(9,9)) > 1E6
        nextP(8:9,:) = P(8:9,:);
        nextP(:,8:9) = P(:,8:9);
    end
    
    % Force symmetry on the covariance matrix to prevent ill-conditioning
    % of the matrix which would cause the filter to blow-up
    for i = 2:16
        for j = 1:(i-1)
            temp = 0.5*(nextP(i,j) + nextP(j,i));
            nextP(i,j) = temp;
            nextP(j,i) = temp;
        end
    end
   
end